package com.rockwell.sniffyhunter.model;

import com.rockwell.sniffyhunter.utils.IRobotPositionDetector;
import com.rockwell.sniffyhunter.utils.IRobotSensors;
import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.MazeWallAlignerState;

/**
 * This class provides the robot for aligning with the maze wall
 * The following actions will be performed:
 * <ul>
 * <li>Turn the robot slowly to align its body with the maze wall.
 * </ul>
 * <p>
 * 
 * @author      Sniffy Software Team
 * @version     1.0
 */

public class MazeWallAligner extends StateObject {
	
	private static final int TURN_INDEX = 10; //Degree
	private static final int MOVE_INDEX = 20; //mm
	private static final int MOVE_BACK_INDEX = 10; //mm
	private static final int WALL_DISTANCE_DIFFERENCE_LIMIT = 1000;
	private static final int WALL_DISTANCE_DIFFERENCE_LIMIT2 = 1500;
	private static final int MaxIndexCount = 55; // Max 270 degree
	private int turnDirection = 1;
	
	private RobotCommandSequencer m_commandSequencer;
	private RobotPosition m_robotPosition;
	private IRobotSensors m_sensors;
	private IRobotPositionDetector m_positionDetector;

	private ExitCode m_exitCode;
	private enum ExitCode {
		NOT_STARTED,
		TURN_DONE,
		NO_WALL_AFTER_TURN
	}
	
	public MazeWallAligner(Robot robot) {
		super(robot);
		m_commandSequencer = new RobotCommandSequencer(robot);
		m_positionDetector = robot.m_positionDetector;
		m_sensors = robot.m_sensors;
		m_exitCode = ExitCode.NOT_STARTED;
	}
	
	@Override
	public void Initialize() {
		//m_commandSequencer.Initialize();
		machineState = MazeWallAlignerState.START;
		machineRecoveryState = machineState;
	}
	
	@Override
	public boolean IsDone() {
		return machineState == MazeWallAlignerState.DONE;
	}
	
	public boolean isNoWallAfterTurn() {
		return (m_exitCode == ExitCode.NO_WALL_AFTER_TURN);
	}

	public IStateMachine start() {
		m_exitCode = ExitCode.NOT_STARTED;
		machineState = MazeWallAlignerState.CHECK_FRONT_WALL_PRESENT;
		return machineState;
	}
	
	public IStateMachine checkFrontWallPresent() {
		m_robotPosition = m_positionDetector.getPosition();

		if (m_robotPosition.frontwalldetected) {
			log("frontwalldetected = true.MazeWallAligner", 4);
			machineState = MazeWallAlignerState.CHECK_ALIGN_FRONT_DONE;
		}
		else {
			log("frontwalldetected = false.MazeWallAligner", 4);
			machineState = MazeWallAlignerState.MOVE_FORWARD;
		}
		
		return machineState;
	}
	
	public IStateMachine moveForward() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveForward, MOVE_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("moveForward.MazeWallAligner", 4);
			machineState = MazeWallAlignerState.CHECK_FRONT_WALL_PRESENT;
		}
		
		return machineState;
	}
	
	public IStateMachine checkAlignFrontDone() {
		m_robotPosition = m_positionDetector.getPosition();
		int[] wallDistances = m_sensors.getFrontDistances();
		
		if (m_robotPosition.front1walldetected && Math.abs(wallDistances[0] - wallDistances[1]) <= WALL_DISTANCE_DIFFERENCE_LIMIT) {
			turnDirection = 1;
			
			if (m_robotPosition.isFrontCloseOrTooClose())
				machineState = MazeWallAlignerState.REVERSE_BEFORE_ALIGN_SIDE;
			else
				machineState = MazeWallAlignerState.ALIGN_SIDE;
		}
		else if (m_robotPosition.front3walldetected && Math.abs(wallDistances[1] - wallDistances[2]) <= WALL_DISTANCE_DIFFERENCE_LIMIT) {
			turnDirection = -1;
			
			if (m_robotPosition.isFrontCloseOrTooClose())
				machineState = MazeWallAlignerState.REVERSE_BEFORE_ALIGN_SIDE;
			else
				machineState = MazeWallAlignerState.ALIGN_SIDE;
		}
		else if (m_robotPosition.front2walldetected && !m_robotPosition.front1walldetected && !m_robotPosition.front3walldetected) {
			
			if (wallDistances[0] + WALL_DISTANCE_DIFFERENCE_LIMIT > wallDistances[2])
				turnDirection = 1;
			else
				turnDirection = -1;
			
			if (m_robotPosition.isFrontCloseOrTooClose())
				machineState = MazeWallAlignerState.REVERSE_BEFORE_ALIGN_SIDE;
			else
				machineState = MazeWallAlignerState.ALIGN_SIDE;
		}
		else if (!m_robotPosition.frontwalldetected) {
			machineState = MazeWallAlignerState.CHECK_FRONT_WALL_PRESENT;
		}
		else if (wallDistances[0] + WALL_DISTANCE_DIFFERENCE_LIMIT > wallDistances[2]) {
			turnDirection = -1;
			machineState = MazeWallAlignerState.ALIGN_FRONT;
		}
		else {
			turnDirection = 1;
			machineState = MazeWallAlignerState.ALIGN_FRONT;
		}

		return machineState;
	}
	
	public IStateMachine alignFront() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, turnDirection * TURN_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("alignFront.MazeWallAligner", 4);
			machineState = MazeWallAlignerState.CHECK_ALIGN_FRONT_DONE;
		}
		
		return machineState;
	}
	
	public IStateMachine reverseBeforeAlignSide() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveBackward, MOVE_BACK_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("reverseBeforeAlignSide.MazeWallAligner", 4);
			machineState = MazeWallAlignerState.ALIGN_SIDE;
		}
		
		return machineState;
	}
	
	public IStateMachine checkAlignSideDone() {
		
		m_robotPosition = m_positionDetector.getPosition();
		
		if (turnDirection == 1) {
			if (m_robotPosition.leftwall2detected && !m_robotPosition.leftwalldetected) {
				log("robot protruding.MazeWallAligner", 4);
				machineState = MazeWallAlignerState.REVERSE_DUE_TO_ROBOT_PROTRUDING;
				return machineState;
			}
			
			machineState = MazeWallAlignerState.DONE;
		}
		else {
			if (m_robotPosition.rightwall2detected && !m_robotPosition.rightwalldetected) {
				log("robot protruding.MazeWallAligner", 4);
				machineState = MazeWallAlignerState.REVERSE_DUE_TO_ROBOT_PROTRUDING;
				return machineState;
			}
			
			machineState = MazeWallAlignerState.DONE;
		}

		return machineState;
	}
	
	public IStateMachine alignSide() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, turnDirection * 90);

		if (returnVal == ReturnResult.Completed) {
			log("alignSide.MazeWallAligner", 4);
			machineState = MazeWallAlignerState.CHECK_ALIGN_SIDE_DONE;
		}
		
		return machineState;
	}
	
	public IStateMachine reverseDueToRobotProtruding() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveBackward2, MOVE_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("reverseDueToRobotProtruding.MazeWallAligner", 4);
			machineState = MazeWallAlignerState.CHECK_REVERSE_DUE_TO_ROBOT_PROTRUDING_DONE;
		}
		
		return machineState;
	}
	
	public IStateMachine checkReverseDueToRobotProtrudingDone() {
		m_robotPosition = m_positionDetector.getPosition();
		
		if (m_robotPosition.leftwalldetected || m_robotPosition.rightwalldetected) {
			log("checkReverseDueToRobotProtrudingDone = true.MazeWallAligner", 4);
			machineState = MazeWallAlignerState.CHECK_ALIGN_SIDE_DONE;
		}
		else {
			machineState = MazeWallAlignerState.REVERSE_DUE_TO_ROBOT_PROTRUDING;
		}
		
		return machineState;
	}
}